/*
 * manual_control_server.cpp
 *
 *  Created on: Jan 24, 2013
 *      Author: michiel
 */

#include "servers/manual_control_server.h"

ManualControl::ManualControl(std::string name) : BaseServer(name),
	_trunk_up_button(false),
	_trunk_down_button(false),
	_trunk_neutral_button(false),
	_gripper_open_button(false),
	_gripper_closed_button(false),
	_server_stop(false),
	_as(nh_, name, boost::bind(&ManualControl::execute, this, _1), false)
{
	_end_time = ros::Time::now();
	controller_sub_ = nh_.subscribe(Topic::PS3_JOY, 10, &ManualControl::controllerCallback, this);
	eyes_pitch_pub_ = nh_.advertise<std_msgs::Float64>(Topic::EYES_PITCH, 10);
	eyes_left_pub_	= nh_.advertise<std_msgs::Float64>(Topic::EYES_LEFT_YAW, 10);
	eyes_right_pub_	= nh_.advertise<std_msgs::Float64>(Topic::EYES_RIGHT_YAW, 10);

	std_msgs::Float64 speed;
	speed.data = 0.01;

	ros::Publisher ps = nh_.advertise<std_msgs::Float64>("/tilt_controller/set_speed", 10);
	ros::Publisher ls = nh_.advertise<std_msgs::Float64>("/left_controller/set_speed", 10);
	ros::Publisher rs = nh_.advertise<std_msgs::Float64>("/right_controller/set_speed", 10);

	ps.publish(speed);
	ls.publish(speed);
	rs.publish(speed);

	_as.start();
}

ManualControl::~ManualControl() {};

void ManualControl::controllerCallback(const sensor_msgs::Joy::ConstPtr &msg) {
	if (msg->buttons[PS3_BUTTON_CROSS_UP] == 1) {
		_trunk_up_button = true;
	}
	if (msg->buttons[PS3_BUTTON_CROSS_DOWN] == 1) {
		_trunk_down_button = true;
	}
	if (msg->buttons[PS3_AXIS_BUTTON_ACTION_TRIANGLE] == 1) {
		_trunk_neutral_button = true;
	}
	if (msg->buttons[PS3_BUTTON_ACTION_CROSS] == 1) {
		_gripper_closed_button = true;
	}
	if (msg->buttons[PS3_BUTTON_ACTION_TRIANGLE] == 1) {
		_gripper_open_button = true;
	}
	if (msg->buttons[PS3_BUTTON_REAR_LEFT_1] == 1 && msg->buttons[PS3_BUTTON_REAR_RIGHT_1] == 1) {
		_server_stop = true;
	}

	// Eyes pitch dynamixels move from 0.0 to 0.4
	// Do not change or we demolish Mart's construction!
	_eyes_pitch = (msg->axes[PS3_AXIS_STICK_LEFT_UPWARDS] + 1.0) / 5.0;
	_eyes_yaw = msg->axes[PS3_AXIS_STICK_LEFT_LEFTWARDS] / 2.5;
}

void ManualControl::execute(const rp_states::ManualControlGoalConstPtr &goal) {
	_server_stop = false;
	result_.done = false;
	while (ros::ok() && !(_as.isPreemptRequested()) && _as.isActive()) {
		if (ros::Time::now() > _end_time) {
			std_msgs::Float64 pitch_msg;
			std_msgs::Float64 yaw_msg;

			pitch_msg.data 	= _eyes_pitch;
			yaw_msg.data	= _eyes_yaw;

			eyes_pitch_pub_.publish(pitch_msg);
			eyes_left_pub_.publish(yaw_msg);
			eyes_right_pub_.publish(yaw_msg);

			_end_time = ros::Time::now() + ros::Duration(0.01);
		}

		// Handle trunk commands.
		if (_trunk_neutral_button || _trunk_down_button || _trunk_up_button) {
			comm::trunk msg;
			if (_trunk_neutral_button) {
				msg.state = TRUNK_UP;
			} else if (_trunk_up_button) {
				msg.state = TRUNK_NEUTRAL;
			} else {
				msg.state = TRUNK_DOWN;
			}
			publishTrunk(msg);
			_trunk_up_button		= false;
			_trunk_down_button		= false;
			_trunk_neutral_button	= false;
		}
		if (_gripper_closed_button || _gripper_open_button) {
			comm::gripper msg;
			if (_gripper_open_button) {
				msg.state = GRIPPER_OPEN;
			} else {
				msg.state = GRIPPER_CLOSED;
			}
			publishGripper(msg);
			_gripper_closed_button	= false;
			_gripper_open_button	= false;
		}
		if (_server_stop) {
			result_.done = true;
			_as.setAborted();
			return;
		}
	}
	_as.setAborted();
}
